/*
 * i2c.c
 *
 * Created: 28/03/2014 02:17:59
 *  Author: Jason
 */ 
#include <util/delay.h>
#include "CoreC.h"




void i2cSetup()
{
      i2c_init();
}

void readAllI2CData()
{
  if(i2c_start(I2CADDRESS+I2C_READ))
  {
    //fail
    blink(1<<LED2,1,blinkSpeed_125,blinkSpeed_125);
  }
  else
  {
    uint8_t LeftMSBData = (i2c_readAck()); //MSB
    uint8_t LeftLSBData = (i2c_readAck()); //LSB
    uint8_t RightMSBData = (i2c_readAck()); //MSB
    uint8_t RightLSBData = (i2c_readNak()); //LSB
    i2c_stop();
    URobotDistance.robotDistance.LeftDistance = (LeftMSBData<<8)|LeftLSBData;
    URobotDistance.robotDistance.RightDistance = (RightMSBData<<8)|RightLSBData;
    //return LSBData;
    //return (MSBData<<8)|LSBData;
  }  

}

void writeAllI2CData()
{
  if (i2c_start(I2CADDRESS+I2C_WRITE))
  {
    //fail
    blink(1<<LED2,1,blinkSpeed_125,blinkSpeed_125);
  }
  else
  {
    i2c_write(lineSensorData.sensor1);
    i2c_write(lineSensorData.sensor2);
    i2c_write(lineSensorData.sensor3);
    i2c_write(lineSensorData.sensor4);
    i2c_write(lineSensorData.sensor5);
    i2c_write(linePos);
    
    i2c_write(robotSpeed.LeftSpeed); 
    i2c_write(robotSpeed.RightSpeed); 
    i2c_stop();
    
  } 

}

